package com.jwaoo.bluetooth;


import java.math.BigDecimal;
import java.util.LinkedList;
import java.util.Queue;

import android.util.Log;


/**
 * 蓝牙数据处理器
 * @author Charles
 *
 */
public class BTBandDataProcess {
	

	//TAG
	private static final String TAG = BTBandDataProcess.class.getSimpleName();
	
	private static final int BTSourceMax = 21;
	
	//cache最大缓存
	private static final int BTDataCacheMax = 40;
	
	
	
	//原始数据
	private Queue<Double> mBTSource;
	
	//滤波后的数据
	private Queue<Double> mBTFilter;
	
	//数据cache
	private Queue<Double> mBTCache;
	
	@SuppressWarnings("unused")
	private Queue<Double> mVSpeed;
	@SuppressWarnings("unused")
	private static final int VSpeedCacheMax = 30;
	private static final Double MinDouble = -Double.MAX_VALUE;
	private static final Double MaxDouble = Double.MAX_VALUE;
	private Double MaxVSpeed = MinDouble;
	private Double MinVSpeed = MaxDouble;
	//最大加速度
	private static final double MaxAccel = -1000.0;
	
	//最小加速度
	private static final double MinAccel = 1000.0;
	
	//波峰
	private double mPeak = 0.0;
	
	//波谷
	private double mTrough = 0.0;
	
	//波峰波谷的落差阈值
	private static final double gapThreshold = 0.01;
	
	//最大深度
	private static final double maxDepth = 1.0;
	
	//方差阈值
	private static final double VarianceThreshold = 0.0001;
	
	
	//方差
	private double mVariance = 0.0;
	
	//平均值
	private double mAverage = 0.0;
	
	
	//上次深度
	private double lastDepth = 0.0;
	
	//上次加速度
	private double lastAccelX = 0.0;
	
	//曲线上升
	private boolean lastCurveUp = false;
	
	//频率
	@SuppressWarnings("unused")
	private double freq = 0.0;
	
	//频率阈值
	//private static final float freqThreshold = 8.0f;
	//上次时间
	private long lastTime = 0;
	
	//低通滤波系数
//	private static final double[] nb = new double[]{7.136407237601E-9,1.42728144752E-7,1.355917375144E-6,8.135504250865E-6,3.457589306618E-5,0.0001106428578118,0.0002766071445294,0.0005532142890588,0.0008989732197206, 0.001198630959627,  0.00131849405559, 0.001198630959627,0.0008989732197206,0.0005532142890588,0.0002766071445294,0.0001106428578118,3.457589306618E-5,8.135504250865E-6,1.355917375144E-6,1.42728144752E-7,7.136407237601E-9};
	
//	private static final double[] na = new double[]{0.0,   -7.194091835284,    26.62308164609,   -65.98963230126,121.5313314139,   -175.4744068797,    205.0088928294,   -197.6510906762,159.1947628639,   -107.8907620074,    61.72859748629,   -29.81215019494,12.11309332249,   -4.112484018429,    1.153860006913,   -0.263149759325,0.04759251946719,-0.006572922826265,0.0006516320791281,-4.131947430844E-5,1.259513198204E-6};
	
	
	private static final double[] nb = new double[]{ 2.867384400357e-012,5.734768800714e-011,5.448030360678e-010,3.268818216407e-009,
		  1.389247741973e-008,4.445592774313e-008,1.111398193578e-007,2.222796387157e-007,
		  3.61204412913e-007,4.81605883884e-007,5.297664722723e-007,4.81605883884e-007,
		  3.61204412913e-007,2.222796387157e-007,1.111398193578e-007,4.445592774313e-008,
		  1.389247741973e-008,3.268818216407e-009,5.448030360678e-010,5.734768800714e-011,
		  2.867384400357e-012};
	
	private static final double[] na = new double[]{                0,   -11.99378017162,    69.41165644869,   -257.3693258566,
	      684.9148471655,    -1389.21626524,    2226.486426182,    -2885.15331847,
	      3068.102191284,   -2702.317142789,    1981.137693847,   -1210.501541604,
	      615.1008807534,   -258.4187123042,     88.8556606349,   -24.61260608736,
	      5.361835809716,  -0.8851266245437,   0.1041357771743,-0.007783676685313,
	  0.0002779267153158};
	
	public BTBandDataProcess(){
		
		mBTCache = new LinkedList<Double>();
		
		mBTSource = new LinkedList<Double>();
		
		mBTFilter = new LinkedList<Double>();
		
		mVSpeed = new LinkedList<Double>();
	}
	

	
	/**
	 * 获取当前深度 0-1之间
	 * @param accelX
	 * @return
	 */
	double prevX = 0;
	double prevV = 0;
	double V = 0;
	double S = 0;
	double T = 0.010;
	double alpha = 0.6;
	double gravity = 0;	
	static final int max_cache = 5;
	double PrevUpS = 0;
	double PrevDownS = 0;
	double mCacheX[] = new double[max_cache];
	int dir = -1;
	double PrevS = 0;
	static final double min_s = 0.05;
	double fileredX = 0;
	static final double filter = 0;
	
	private int Countbiger(double v, int index){
		int count = 0;
		for (int i = index+1 ; i < max_cache; i++)
		{
			if (mCacheX[i] > v){
				count ++;
			}
		}
		return count;
	}
	
	private int Countless(double v, int index){
		int count = 0;
		for (int i = index+1 ; i < max_cache; i++)
		{
			if (mCacheX[i] < v){
				count ++;
			}
		}
		return count;
	}
	private int mCount = 0;
	private double m_depth = 0;
	
	private double getVSpeed(double accelX){
		gravity = alpha * gravity + (1 - alpha) * accelX;
		accelX = accelX - gravity;
		//Log.e(TAG, "gravity="+gravity+", prevX="+prevX+" accelX="+accelX);
		//fileredX = fileredX * filter + (1-filter)*accelX;
		
		V = V + accelX * T;
		//BigDecimal d = new BigDecimal(V);
		//V = d.setScale(5,BigDecimal.ROUND_HALF_UP).doubleValue();
		
		//S = S + V * T + accelX * T * T / 2;
		
		return V;
	}
	
	@SuppressWarnings("unused")
	private double roundDepth(double depth){
		BigDecimal d = new BigDecimal(depth);
		return d.setScale(4,BigDecimal.ROUND_HALF_UP).doubleValue();		
	}
	
	@SuppressWarnings("unused")
	private double CalcDepth(double accelX){
		gravity = alpha * gravity + (1 - alpha) * accelX;
		accelX = accelX - gravity;
		//Log.e(TAG, "gravity="+gravity+", prevX="+prevX+" accelX="+accelX);
		fileredX = fileredX * filter + (1-filter)*accelX;
		//accelX = fileredX;
		//if (MaxVSpeed > accelX)
		prevX = fileredX;
		
		V = V + fileredX * T;
		BigDecimal d = new BigDecimal(V);
		V = d.setScale(5,BigDecimal.ROUND_HALF_UP).doubleValue();
		//V = Double.
		//mVSpeed.add(V);
		if (MinVSpeed > V){
			 MinVSpeed = V;
		}
		if (MaxVSpeed < V){
			MaxVSpeed = V;
		}		
		for (int i = 1; i < max_cache; i++){
			mCacheX[i-1] = mCacheX[i];
		}
		mCacheX[max_cache-1] = V;
		mCount ++;
		if (mCount < max_cache)
			return m_depth;
		//if (MaxVSpeed - MinVSpeed < 0.01)
			///return m_depth;
		Log.e(TAG, "V="+V+", fileredX="+fileredX);
		if (dir != 1 && mCacheX[0] > mCacheX[1] && mCacheX[1] > mCacheX[2] && mCacheX[2] < mCacheX[3]){
			int count = Countbiger(mCacheX[2], 2);
			if (count == 2) {
				Log.e(TAG, "----change to up, dltaS="+S);
				PrevUpS = S;
				S = 0;
				//V = 0;
				dir = 1;
				MinVSpeed = MaxDouble;
			}else{
				Log.e(TAG, "***Down:filter S="+S+", PrevUpS="+PrevUpS);
			}
		}
		if (dir != 2 && mCacheX[0] < mCacheX[1] && mCacheX[1] > mCacheX[2] && mCacheX[2] > mCacheX[3]){
			int count = Countless(mCacheX[2], 2);
			if (count == 2){
				Log.e(TAG, "----change to down, dltS="+S);
				PrevDownS = S;
				S = 0;
				//V = 0;
				dir = 2;
				MaxVSpeed = MinDouble;
			}else{
				Log.e(TAG, "***Up:filter S="+S+", PrevDownS="+PrevUpS);
			}
		}
		PrevS = S;
		double prevVVV = mCacheX[2];
		S = S + prevVVV * T + fileredX * T * T / 2;
		//Log.e(TAG, "pV="+prevVVV+", S="+S);
		
		if (dir == 1) {
			double t = S/PrevUpS;
			t = t > 1.0?1.0:t;
			m_depth = t;
			m_depth = 1 - m_depth;
			Log.e(TAG, "depth_new="+m_depth);
		}
		if (dir == 2) {
			double t = S/PrevDownS;
			t = t > 1.0?1.0:t;
			m_depth = t;
			Log.e(TAG, "depth_new="+m_depth);
		}
		return m_depth;
		
	}
	

	public void calcS(double accelX){
		gravity = alpha * gravity + (1 - alpha) * accelX;
		accelX = accelX - gravity;
		//Log.e(TAG, "gravity="+gravity+", prevX="+prevX+" accelX="+accelX);
		if (prevX > 0 && accelX < 0){
			Log.e(TAG, "chnage to down "+" V="+V+" S="+S);
			//V = 0;
		}
		if (prevX < 0 && accelX > 0){
			Log.e(TAG, "change to up "+" V="+V+" S="+S);
			//S = 0;
			//V = 0;
		}
		prevX = accelX;
		S = S + V * T + accelX * T * T / 2;
		V = V + accelX * T;
		Log.e(TAG, "V="+V+", S="+S+", dltV="+(accelX * T));
//		if (prevV > 0 && V < 0){
//			Log.e(TAG, "chnage to down "+" V="+V+" S="+S);
//			V = 0;
//			S = 0;
//		}
//		if (prevV < 0 && V > 0){
//			Log.e(TAG, "chnage to up "+" V="+V+" S="+S);
//			V = 0;
//			S = 0;			
//		}
//		prevV = V;
	}
	public double[] getCurDepth(double accelX,boolean isFilter){
		
		accelX = isFilter?filterData(accelX):accelX;			
		
		//Log.e(TAG, "accelx="+(accelX));
		//change the directions
		accelX = 0 - accelX;
		//double d = CalcDepth(accelX);
		accelX = getVSpeed(accelX);
		//滤波后数据
			
		//是否计算波峰波谷
		boolean shouldCalPeakAndTrough = true;
		if(mBTCache.size()>=BTDataCacheMax){
			double head = mBTCache.poll();
			if(head!=mPeak&&head!=mTrough){
				shouldCalPeakAndTrough = false;
			}
		}
		
		//计算波峰波谷
		if(shouldCalPeakAndTrough){
			calPeakAndTrough();			
		}
		mBTCache.add(accelX);
		if(accelX>mPeak){
			mPeak = accelX;
		}
		if(accelX<mTrough){
			mTrough = accelX;
		}
		double gap = mPeak - mTrough;
		double depth = lastDepth;

		if(mVariance>VarianceThreshold){
			depth = (accelX - mTrough)/gap;	
			depth = maxDepth - depth;
		}else{//缓慢起作用
			if(gap>gapThreshold)
			{
				depth = (accelX + gap*0.5 - mAverage)/gap;
				depth = maxDepth - depth;
			}
			else
			{
			}
		}
		depth = depth>1.0?1.0:depth;
		depth = depth<0?0.0:depth;
		boolean curCurveUp = accelX-lastAccelX>=0.0;
		if(!curCurveUp&&lastCurveUp)
		{
			long curTime = System.currentTimeMillis();
			freq = 1000.0/(curTime-lastTime);
			lastTime = curTime;
		}
		lastAccelX = accelX;
		lastCurveUp = curCurveUp;
		lastDepth = depth;

		return new double[]{depth,mVariance};
	}
	

	/**
	 * 低通滤波后数据
	 * @param accelX
	 * @return
	 */
	private double filterData(double accelX)
	{
		if(mBTSource.size()>=BTSourceMax){
			
			mBTSource.poll();
			
		}
		
		mBTSource.add(accelX);
		
		Object[] arr = mBTSource.toArray();
		
		Object[] fiterArr = mBTFilter.toArray();
		
		
		
		int srcLen = mBTSource.size();
		
	//	int fiterLen = mBTFilter.size();
		
		
		double value = 0.0;
		
		double accelVal = 0.0;
		
		double fiterVal = 0.0;
		
		for(int i = 0;i<srcLen;i++){
			
			if(i!=0)
			{
				fiterVal = ((Double)fiterArr[srcLen-i-1]).doubleValue();	
						
				
			}
			
			accelVal = ((Double)arr[srcLen-i-1]).doubleValue();
			
			
			value = value + nb[i]*accelVal - na[i]*fiterVal;
			
		//	Log.e(TAG, "fiterArr size="+fiterLen+" srcLen="+srcLen+" i="+i+" index="+(srcLen-i-1)+" fiterVal="+fiterVal+" accelVal="+accelVal);
		}
		
	//	Log.e(TAG, "totalValue="+value);
		
		if(mBTFilter.size()>=BTSourceMax-1){
			
			mBTFilter.poll();
			
		}
		mBTFilter.add(value);
		
		
		return value;
	}
	
	
	/**
	 * 计算波峰波谷
	 */
	private void calPeakAndTrough(){
		
		
		
		Object[] arr = mBTCache.toArray();
		
		mPeak = MaxAccel;
		
		mTrough = MinAccel;
		
		mAverage = 0.0;
		
		mVariance = 0.0;
		
		for(Object obj:arr){
					
			double value = ((Double)obj).doubleValue();
			
			mAverage += value;
			
			mVariance += value*value;
			
			if(value>mPeak){
				
				mPeak = value;
			}
			
			if(value<mTrough){
				
				mTrough = value;
			}
		}
		
		mAverage /= arr.length;
		
		mVariance = (mVariance-arr.length*mAverage*mAverage)/arr.length;
		

	}


}
